# -*- encoding:UTF-8 -*-
from naoqi import ALProxy
import cmath
import time

IP = "192.168.8.101"
PORT = 9559

if __name__ == '__main__':
    # 直接到最终动作
    maxstepx = 0.02
    maxstepy = 0.03
    maxsteptheta = 0.1
    maxstepfrequency = 0.2
    maxstepheight = 0.05
    torsowx = 0.0
    torsowy = 0.0

    moveConfig = [["MaxStepX", maxstepx],
                  ["MaxStepY", maxstepy],
                  ["MaxStepTheta", maxsteptheta],
                  ["StepHeight", maxstepheight],
                  ["MaxStepFrequency", maxstepfrequency],
                  ["TorsoWx", torsowx],
                  ["TorsoWy", torsowy]]

    motionProxy = ALProxy("ALMotion", IP, PORT)
    motionProxy.wakeUp()

    # 收回手臂
    motionProxy.setAngles("RShoulderRoll", -75.5 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RWristYaw", 73.3 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderPitch", 73 * cmath.pi / 180, 0.1)
    time.sleep(3)
    motionProxy.setAngles("RShoulderRoll", -13 * cmath.pi / 180, 0.1)
